/************************************************************************************
 * WrightEagle (Soccer Simulation League 2D)                                        *
 * BASE SOURCE CODE RELEASE 2010                                                    *
 * Copyright (c) 1998-2010 WrightEagle 2D Soccer Simulation Team,                   *
 *                         Multi-Agent Systems Lab.,                                *
 *                         School of Computer Science and Technology,               *
 *                         University of Science and Technology of China            *
 * All rights reserved.                                                             *
 *                                                                                  *
 * Redistribution and use in source and binary forms, with or without               *
 * modification, are permitted provided that the following conditions are met:      *
 *     * Redistributions of source code must retain the above copyright             *
 *       notice, this list of conditions and the following disclaimer.              *
 *     * Redistributions in binary form must reproduce the above copyright          *
 *       notice, this list of conditions and the following disclaimer in the        *
 *       documentation and/or other materials provided with the distribution.       *
 *     * Neither the name of the WrightEagle 2D Soccer Simulation Team nor the      *
 *       names of its contributors may be used to endorse or promote products       *
 *       derived from this software without specific prior written permission.      *
 *                                                                                  *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND  *
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED    *
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE           *
 * DISCLAIMED. IN NO EVENT SHALL WrightEagle 2D Soccer Simulation Team BE LIABLE    *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL       *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR       *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER       *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,    *
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF *
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                *
 ************************************************************************************/

#include "BehaviorPosition.h"
#include "Agent.h"
#include "Strategy.h"
#include "InterceptInfo.h"
#include "Dasher.h"
#include "VisualSystem.h"
#include "BehaviorPass.h"
#include "Logger.h"
#include "TimeTest.h"
#include <sstream>
#include "Evaluation.h"


using namespace std;

const BehaviorType BehaviorPositionExecuter::BEHAVIOR_TYPE = BT_Position;

namespace {
bool ret = BehaviorExecutable::AutoRegister<BehaviorPositionExecuter>();
}


BehaviorPositionExecuter::BehaviorPositionExecuter(Agent & agent) :
	BehaviorExecuterBase<BehaviorAttackData> (agent)
{
	Assert(ret);
}

BehaviorPositionExecuter::~BehaviorPositionExecuter(void)
{
}

bool BehaviorPositionExecuter::Execute(const ActiveBehavior & beh)
{
	Logger::instance().LogGoToPoint(mSelfState.GetPos(), beh.mTarget, "@Position");
  std::cout<<"dgk execute_position"<<std::endl;
	return Dasher::instance().GoToPoint(mAgent, beh.mTarget, beh.mBuffer, beh.mPower, false, true);
}

BehaviorPositionPlanner::~BehaviorPositionPlanner()
{
}

BehaviorPositionPlanner::BehaviorPositionPlanner(Agent &agent) :
	BehaviorPlannerBase<BehaviorAttackData> (agent)
{
}


void BehaviorPositionPlanner::Plan(ActiveBehaviorList &behavior_list)
{
	if (!behavior_list.empty()) return;

	if (mSelfState.IsGoalie()) return;
	if (mStrategy.IsOppControl()) return;
	if (mWorldState.GetPlayMode() > PM_Opp_Mode) return;

  Vector initialPosition=mSelfState.GetPos();
  std::cout<<"dgk initial_position_position "<<initialPosition.X()<<" "<<initialPosition.Y()<<std::endl;

	ActiveBehavior position(mAgent, BT_Position);
	Vector target;
	position.mBuffer = 1.0;
	target = mStrategy.GetTeammateSBSPPosition(mSelfState.GetUnum(), mBallState.GetPos());
	if(mPositionInfo.GetTeammateWithBall() && mPositionInfo.GetClosestPlayerToTeammate(mPositionInfo.GetTeammateWithBall()) == mSelfState.GetUnum()){
		if((mWorldState.GetOpponent(mPositionInfo.GetClosestOpponentToTeammate(mSelfState.GetUnum())).GetPos()-
				mSelfState.GetPos()).Mod() >= 1.0)
		position.mTarget = target;
		else {
			position.mTarget.SetX(target.X() + 1.0);
			position.mTarget.SetY(target.Y());
		}

	}
	else position.mTarget = target;

	if(mWorldState.GetPlayMode() == PM_Our_Goalie_Free_Kick  &&
			mAgent.GetFormation().GetMyRole().mLineType == LT_Forward){
		if(mAgent.GetFormation().GetMyRole().mPositionType ==PT_Left){
			position.mTarget.SetY(26);
		}
		if(mAgent.GetFormation().GetMyRole().mPositionType ==PT_Right){
			position.mTarget.SetY(-26);
		}
	}

	Unum opp =mPositionInfo.GetClosestOpponentToPoint(position.mTarget);
	if(mWorldState.GetOpponent(opp).GetPos().Dist(position.mTarget) < 1.5){
			int p =mWorldState.GetOpponent(mPositionInfo.GetClosestOpponentToPlayer(opp)).GetPos().Y() > mWorldState.GetOpponent(opp).GetPos().X() ? -1:1;
			position.mTarget.SetY(target.Y() + p * 2);
	}

	if(mPositionInfo.GetOpponentOffsideLine() > mBallState.GetPos().X()){
		position.mTarget.SetX(Min(target.X(),(mPositionInfo.GetOpponentOffsideLine())));
	}
	if(position.mTarget.X()> mSelfState.GetPos().X() && mAgent.GetFormation().GetMyRole().mLineType == LT_Forward){
	position.mPower = mSelfState.CorrectDashPowerForStamina(ServerParam::instance().maxDashPower());
	}
	else 	if(position.mTarget.X()< mSelfState.GetPos().X() && mAgent.GetFormation().GetMyRole().mLineType == LT_Defender){
		position.mPower = mSelfState.CorrectDashPowerForStamina(ServerParam::instance().maxDashPower());
	}
	else position.mPower = mSelfState.CorrectDashPowerForStamina(ServerParam::instance().maxDashPower())/2;
  position.mEvaluation = Evaluation::instance().EvaluatePosition(position.mTarget, false);

  std::cout<<"dgk planned_position "<<position.mTarget.X()<<" "<<position.mTarget.Y()<<std::endl;
  std::cout<<"dgk planned_position_evaluation "<<position.mEvaluation<<std::endl;
  //force fields
  double di_ball=dist(mSelfState.GetPos().X(), mSelfState.GetPos().Y(), mBallState.GetPos().X(), mBallState.GetPos().Y());
  double B[2], strengthB=1.0;
  forceFieldB(B, &initialPosition);
  double O[2], strengthO=1.0;
  forceFieldO(O, &initialPosition);
  if(softIf(di_ball, 20, 10)) {
    //close to ball - tactical
    double T[2], strengthT=1.0;
    forceFieldT(T, &initialPosition);

    double C[2], strengthC=1.0;
    forceFieldC(C);

    std::cout<<"dgk strength T "<<strengthT<<" C "<<strengthC<<std::endl;
    position.mTarget.SetX(position.mTarget.X()+strengthB*B[0]+strengthO*O[0]+strengthT*T[0]+strengthC*C[0]);
    position.mTarget.SetY(position.mTarget.Y()+strengthB*B[1]+strengthO*O[1]+strengthT*T[1]+strengthC*C[1]);
  } else {
    //far from ball - strategic
    double S[2], strengthS=1.0;
    forceFieldS(S, &initialPosition);

    std::cout<<"dgk strength B "<<strengthB<<" O "<<strengthO<<" S "<<strengthS<<std::endl;
    position.mTarget.SetX(position.mTarget.X()+strengthB*B[0]+strengthO*O[0]+strengthS*S[0]);
    position.mTarget.SetY(position.mTarget.Y()+strengthB*B[1]+strengthO*O[1]+strengthS*S[1]);
  }
  position.mEvaluation = Evaluation::instance().EvaluatePosition(position.mTarget, false);
  std::cout<<"dgk forcefield_position "<<position.mTarget.X()<<" "<<position.mTarget.Y()<<std::endl;
  std::cout<<"dgk forcefield_position_evaluation "<<position.mEvaluation<<std::endl;
  // -- force fields
	behavior_list.push_back(position);
}

bool BehaviorPositionPlanner::softIf(double left, double right, double delta) {
  int val=(int)floor(0.5+1.0/(1+exp(-(right-left)/delta)));
  return val==0;
}

bool BehaviorPositionPlanner::softEq(double left, double right, double delta) {
  int val=(int)floor(0.5+exp(-pow(left-right, 2.0)/pow(delta, 2.0)));
  return val==0;
}

double BehaviorPositionPlanner::dist(double x1, double y1, double x2, double y2) {
  return sqrt(pow(x1-x2, 2)+pow(y1-y2, 2));
}

void BehaviorPositionPlanner::forceFieldB(double *B, Vector *initialPosition) {
  //B: bounds-repellent force field
  //from WorldState.LimitToField
  //X = from -ServerParam::instance().PITCH_LENGTH/2 to ServerParam::instance().PITCH_LENGTH/2 = -52.5 52.5
  //Y = from -ServerParam::instance().PITCH_WIDTH/2 to ServerParam::instance().PITCH_WIDTH/2 = -34 34
  B[0]=initialPosition->X();
  double pitch_length=ServerParam::instance().PITCH_LENGTH/2;
  //avoid /0
  if(B[0]<=-pitch_length) {
    B[0]=-(pitch_length-0.1);
  }
  else if(B[0]>=pitch_length) {
    B[0]=pitch_length-0.1;
  }
  B[0]=5.0/(B[0]+pitch_length)-5.0/(pitch_length-B[0]);
  B[1]=initialPosition->Y();
  double pitch_width=ServerParam::instance().PITCH_WIDTH/2;
  std::cout<<"dgk debug B length "<<pitch_length<<" width "<<pitch_width<<std::endl;
  //avoid /0
  if(B[1]<=-pitch_width) {
    B[1]=-(pitch_width-0.1);
  }
  else if(B[1]>=pitch_width) {
    B[1]=pitch_width-0.1;
  }
  B[1]=5.0/(B[1]+pitch_width)-5.0/(pitch_width-B[1]);
  //capping
  B[0]=B[0]>5.0 ? 5.0 : B[0];
  B[0]=B[0]<-5.0 ? -5.0 : B[0];
  B[1]=B[1]>5.0 ? 5.0 : B[1];
  B[1]=B[1]<-5.0 ? -5.0 : B[1];
  std::cout<<"dgk position_B "<<B[0]<<" "<<B[1]<<std::endl;
  // -- B: bounds-repellent force field
}

void BehaviorPositionPlanner::forceFieldO(double *O, Vector *initialPosition) {
  //O: offsides-repellent force field
  //if(mPositionInfo.GetOpponentOffsideLine() > mBallState.GetPos().X()){
  //    position.mTarget.SetX(Min(target.X(),(mPositionInfo.GetOpponentOffsideLine())));
  //  }
  O[0]=0.0;
  O[1]=0.0;
  //less than 5m from offside line
  std::cout<<"dgk debug O offsideline "<<mPositionInfo.GetOpponentOffsideLine()<<std::endl;
  if(softIf(abs(mPositionInfo.GetOpponentOffsideLine()-initialPosition->X()), 5.0, 1.0)) {
    //which way shoul I go?
    double goalieX=mWorldState.GetPlayer(mWorldState.GetTeammateGoalieUnum()).GetPos().X();
    std::cout<<"dgk debug O goalieX "<<goalieX<<std::endl;
    if(goalieX>0) {
      if(initialPosition->X()<mPositionInfo.GetOpponentOffsideLine()) {
        O[0]=5.0;
      }
    } else {
      if(initialPosition->X()>mPositionInfo.GetOpponentOffsideLine()) {
        O[0]=-5.0;
      }
    }
  }
  std::cout<<"dgk position_O "<<O[0]<<" "<<O[1]<<std::endl;
  // -- O: offsides-repellent force field
}

void BehaviorPositionPlanner::forceFieldS(double *S, Vector *initialPosition) {
  //S: inter-player force field
  //mPositionInfo.GetCloseTeammateToPlayer(Unum i);
  //replace closest by GetCloseTeammateToPlayer
  Vector posTeammate=mWorldState.GetPlayer(mPositionInfo.GetClosestTeammateToPlayer(mSelfState.GetUnum())).GetPos();
  double di=dist(initialPosition->X(), initialPosition->Y(), posTeammate.X(), posTeammate.Y());
  std::cout<<"dgk debug S teammate "<<posTeammate.X()<<" "<<posTeammate.Y()<<" di "<<di<<std::endl;
  S[0]=0.0;
  S[1]=0.0;
  if(softEq(di, 20, 10)) {
    S[0]=posTeammate.X()-initialPosition->X();
    S[1]=posTeammate.Y()-initialPosition->Y();
    double norm=sqrt(pow(S[0], 2.0)+pow(S[1], 2.0));
    //2 is the strenght of the field
    S[0]=2*S[0]/norm;
    S[1]=2*S[1]/norm;
    if(softIf(di, 20, 10)) {
      //repel
      S[0]=-S[0];
      S[1]=-S[1];
    }
  }
  std::cout<<"dgk position_S "<<S[0]<<" "<<S[1]<<std::endl;
  // -- S: inter-player force field
}

void BehaviorPositionPlanner::forceFieldT(double *T, Vector *initialPosition) {
  //T: tactical inter-player force field
  T[0]=0.0;
  T[1]=0.0;
  Vector posTeammate=mWorldState.GetPlayer(mPositionInfo.GetClosestTeammateToPlayer(mSelfState.GetUnum())).GetPos();
  double di=dist(initialPosition->X(), initialPosition->Y(), posTeammate.X(), posTeammate.Y());
  if(softIf(di, 8, 3)) {
    T[0]=posTeammate.X()-initialPosition->X();
    T[1]=posTeammate.Y()-initialPosition->Y();
    double norm=sqrt(pow(T[0], 2.0)+pow(T[1], 2.0));
    T[0]=-5.0*T[0]/norm;
    T[1]=-5.0*T[1]/norm;
  }
  std::cout<<"dgk position_T "<<T[0]<<" "<<T[1]<<std::endl;
  // -- T: tactical inter-player force field
}

void BehaviorPositionPlanner::forceFieldC(double *C) {
  //C: get-clear force field
  //closest opponent and ball
  Vector posOpponent=mWorldState.GetPlayer(mPositionInfo.GetClosestOpponentToPlayer(mSelfState.GetUnum())).GetPos();
  Vector posBall=mBallState.GetPos();
  double obx=posOpponent.X()-posBall.X();
  double oby=posOpponent.Y()-posBall.Y();
  //C is orthogonal to the vector defined by Opponent-Ball; dot product = 0
  C[0]=obx;
  C[1]=-obx*C[0]/oby;
  double norm=sqrt(pow(C[0], 2.0)+pow(C[1], 2.0));
  C[0]=C[0]/norm;
  C[1]=C[1]/norm;
  std::cout<<"dgk position_C "<<C[0]<<" "<<C[1]<<std::endl;
  // -- C: get-clear force field
}
